#ifndef __MATH_UTILS_H__
#define __MATH_UTILS_H__

#include <geometry_msgs/PoseStamped.h>
#include <mc_core/common/point.h>

namespace math_utils{

    /************************************
     * 角度、距离计算
     * https://wiki.ros.org/angles
     *************************************/

    // 弧度转角度
    double toDeg(double radians);
    // 角度转弧度
    double toRad(double degrees);
    // 角度转换 [-PI,PI]
    double normalizeAngle(double angle);
    // 角度差 b-a
    double diffAngle(double a, double b);
    // 角度相加 a+b
    double addAngle(double a, double b);

    // 计算两个点之间的角度
    double angleBetweenPoses(geometry_msgs::Pose a, geometry_msgs::Pose b);
    // 计算两个点之间的距离
    double distanceBetweenPoses(geometry_msgs::Pose a, geometry_msgs::Pose b);


}; //math_utils 

#endif